using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Audio;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.GamerServices;
using Microsoft.Xna.Framework.Graphics;
using Microsoft.Xna.Framework.Input;
using Microsoft.Xna.Framework.Net;
using Microsoft.Xna.Framework.Storage;
using System.Threading;
using System.IO.Ports;
using WiimoteLib;

namespace Autobot
{

    public enum RealRobotState : int
    {
        STARTUP = 0,
        PLACE_ROBOT,
        RUN,
        STOP,
        NUM_STATES
    }

    /// <summary>
    /// This is the main type for your game
    /// </summary>
    public class RealRobot : Microsoft.Xna.Framework.Game
    {
        //Graphics device
        GraphicsDeviceManager graphics;

        //Remember the state of the program
        private RealRobotState currentState;

        //A string to write to the window
        //1st line of instructions, second line of instructions
        // and the state strings
        ShowString instructionsString1;
        ShowString instructionsString2;

        //Need to have a map and a filename to save to
       
        private string filename;
        private string communicator_ip_string = null;

        private CommunicatorClient communicator;
        private LogController communicator_log;

        //Information about the last state of keyboard and mouse
        private KeyboardState lastKeyboardInput;
        private MouseState lastMouseInput;

        private List<DelayedMessage> delayedMessages;

        private Thread delayedMessagesThread;

        private DateTime updateServerTime;
        private bool receiveServerMessages;
        private bool requestAlgorithmUpdate;
        private int parsedServerMessages;
        private int expectedServerMessages;

        //SerialPort Communication
        private SerialPort port;

        //Cached Orientation from Wiimote
        private Orientation lastReceivedOrientation;

        //Cached Motor Inputs
        private int turnMotor = (int)SteeringMotorParams.STOP;
        private int driveMotor = (int)DriveMotorParams.STOP;

        public RealRobot(Vector2 WindowSize, string Filename, string ip_string)
        {
            communicator_ip_string = ip_string;
            // Load up the graphics manager
            graphics = new GraphicsDeviceManager(this);

            // Set the window size according to input recieved, leave some room for instructions in the top 20 pixels of screen
            graphics.PreferredBackBufferHeight = (int)WindowSize.Y + 70;
            graphics.PreferredBackBufferWidth = (int)WindowSize.X;
            Window.Title = "Robot Runner";

            //Set the directory for the content
            Content.RootDirectory = "Content";

            //Remember the name of the map file
            filename = Filename;

            //The map will consume most of the window, remember to leave room for the instructions
            Rectangle MapRect = new Rectangle(0, 70, (int)WindowSize.X, (int)WindowSize.Y);

            //This is the first instructions that will be displayed to screen in the top left corner
            instructionsString1 = new ShowString(new Vector2(0f, 0f), "Hit -P To place the robot", 20, Color.Red);
            instructionsString2 = new ShowString(new Vector2(0f, 30f), "", 20, Color.Red);

            
            

            // Create a new logfile for the communicator
            communicator_log = new LogController();
            bool success = communicator_log.OpenLog("ClientLog.txt");
            if (!success)
            {
                Console.WriteLine("ERROR: could not open ClientLog.txt for writing");
                this.Exit();
            }

            //Set up the communicator
            communicator = new CommunicatorClient();

            // Initialize it, quit if it doesn't succeed
            if (!communicator.Initialize(communicator_log, communicator_ip_string))
                this.Exit();

            //Set the current state of the Editor to IDLE
            currentState = RealRobotState.STARTUP;

            // Initialize the list for all delayed commands
            delayedMessages = new List<DelayedMessage>();

            delayedMessagesThread = new Thread(new ThreadStart(this.DelayedMessageThread));
            delayedMessagesThread.Start();

            receiveServerMessages = false;
            requestAlgorithmUpdate = false;
            parsedServerMessages = 0;
            expectedServerMessages = 0;

            //Opening the Serial Port For Communication
            port = new SerialPort("COM4");
            if (port.IsOpen) port.Close();
            port.Open();
            port.DataReceived += new SerialDataReceivedEventHandler(portDataReceived);

            //Connecting the Wiimote 
            Wiimote wm = new Wiimote();
            wm.WiimoteChanged += wm_WiimoteChanged;
            wm.Connect();
            wm.SetReportType(InputReport.IRAccel, true);
        }

        /// <summary>
        /// Allows the game to perform any initialization it needs to before starting to run.
        /// This is where it can query for any required services and load any non-graphic
        /// related content.  Calling base.Initialize will enumerate through any components
        /// and initialize them as well.
        /// </summary>
        protected override void Initialize()
        {
            // Initialize the states to the current keyboard/mouse states
            lastKeyboardInput = Keyboard.GetState();
            lastMouseInput = Mouse.GetState();

            // Make the mouse visible
            this.IsMouseVisible = true;
            base.Initialize();
        }

        /// <summary>
        /// LoadContent will be called once per game and is the place to load
        /// all of your content.
        /// </summary>
        protected override void LoadContent()
        {
            //Load the string content
            instructionsString1.LoadContent(Content, GraphicsDevice);
            instructionsString2.LoadContent(Content, GraphicsDevice);
        }

        /// <summary>
        /// UnloadContent will be called once per game and is the place to unload
        /// all content.
        /// </summary>
        protected override void UnloadContent()
        {
            //Dispose of all graphical resources
            
            instructionsString1.Dispose();
            instructionsString2.Dispose();

            communicator.Close();
            delayedMessagesThread.Abort();
        }

        /// <summary>
        /// Allows the game to run logic such as updating the world,
        /// checking for collisions, gathering input, and playing audio.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Update(GameTime gameTime)
        {
            //Check the current keyboard state
            KeyboardState keyboardInput = Keyboard.GetState();
            MouseState mouseInput = Mouse.GetState();

            //Check if we want to quit
            if (keyboardInput.IsKeyDown(Keys.Escape))
                this.Exit();

            //Enter the large state machine. eep.
            switch ((int)currentState)
            {
                case (int)RealRobotState.STARTUP:

                    // During startup, we should begin with the car stopped.
                    // Only send motor controls if we need to.
                    if (driveMotor != (int)DriveMotorParams.STOP)
                        sendMotorControl((int)MotorID.DRIVE, (int)DriveMotorParams.STOP);

                    // Set the state to run state, kick off the state machine with a request for algorithm update.
                    currentState = RealRobotState.RUN;
                    Message AlgRequest = new Message();
                    AlgRequest.CreateAlgorithmUpdateMsg(0);
                    communicator.SendMessage(0, AlgRequest);
                    requestAlgorithmUpdate = false;
                    break;

                case (int)RealRobotState.RUN:

                    //If we are running, hitting T will stop the car. Check for this.
                    //Only do this once if the key has been pressed.
                    if (keyboardInput.IsKeyDown(Keys.T) && lastKeyboardInput.IsKeyUp(Keys.T))
                    {
                        //SEND STOPPING MESSAGE----------------------
                        if (driveMotor != (int)DriveMotorParams.STOP)
                            sendMotorControl((int)MotorID.DRIVE, (int)DriveMotorParams.STOP);

                        //The car has been stopped with the keystroke. Update the instructions and state.
                        //We change the instruction string now so it doesn't do it every frame.
                        instructionsString1.SetString("Car has now stopped. Press 's' to start again");
                        currentState = RealRobotState.STOP;
                    }

                    // Second, send updates to the server at regular intervals
                    // so that the server does not get flooded.
                    TimeSpan diff = DateTime.Now - updateServerTime;
                    if (diff.Milliseconds > 80)
                    {
                        Message Update = new Message();
                        //RobotData data = robot.GetDataCopy();
                        //Update.CreateUpdateMessage(0, data.realPosition, data.direction, data.sensors, data.motors, data.velocity);
                        communicator.SendMessage(0, Update);
                        updateServerTime = DateTime.Now;
                    }

                    // Third, get the next message from the server and parse it
                    Message nextMessage;
                    nextMessage = communicator.GetNextMessage();
                    ParseMessage(nextMessage);

                    // If the simulator is ready, grab an update
                    if (requestAlgorithmUpdate == true)
                    {
                        Message AlgRequest1 = new Message();
                        AlgRequest1.CreateAlgorithmUpdateMsg(0);
                        communicator.SendMessage(0, AlgRequest1);
                        requestAlgorithmUpdate = false;
                    }
                    break;

                case (int)RealRobotState.STOP:
                    
                    //SEND STOPPING MESSAGE----------------------
                    if( driveMotor != (int)DriveMotorParams.STOP)
                        sendMotorControl((int)MotorID.DRIVE, (int)DriveMotorParams.STOP);

                    //If the user strikes the 's' key, change state and start driving!
                    if (keyboardInput.IsKeyDown(Keys.S) && lastKeyboardInput.IsKeyUp(Keys.S))
                    {
                        // Kick off the messaging structure with a request for an update
                        Message Update = new Message();
                        Update.CreateAlgorithmUpdateMsg(0);
                        communicator.SendMessage(0, Update);

                        if (driveMotor == (int)DriveMotorParams.STOP)
                            sendMotorControl((int)MotorID.DRIVE, (int)DriveMotorParams.MAX);

                        //If we want to continue driving, set motor speed to max and drive
                        // robot.UpdateMotor((int)MotorID.DRIVE, (int)DriveMotorParams.MAX);
                        currentState = RealRobotState.RUN;
                        instructionsString1.SetString("Car is now driving. Press 't' to stop");
                    }
                    break;

                default: throw new Exception("Invalid Virtual Robot State");
            }

            // Remember the current input as the input from the previous update
            lastKeyboardInput = keyboardInput;
            lastMouseInput = mouseInput;

            //Update the base
            base.Update(gameTime);
        }

        //----

        private byte[] messageCache = new byte[0];
        private byte[] appendBytes(byte[] original, byte[] append)
        {
            byte[] ret = new byte[original.Length + append.Length];
            for (int i = 0; original != null && i < original.Length; i++)
                ret[i] = original[i];
            for (int i = 0; i < append.Length; i++)
                ret[original.Length + i] = append[i];
            return ret;
        }
        private byte[] subArray(byte[] array, int start)
        {
            byte[] ret = new byte[array.Length - start];
            for (int i = 0; i < ret.Length; i++)
                ret[i] = array[i];
            return ret;
        }
        private byte[] readExistingBytes(SerialPort port)
        {
            byte[] bytes = new byte[port.BytesToRead];
            for (int i = 0; i < bytes.Length; i++)
                bytes[i] = (byte)port.ReadByte();
            return bytes;
        }
        private void portDataReceived(object sender, SerialDataReceivedEventArgs e)
        {
            messageCache = appendBytes(messageCache, readExistingBytes(port));
            while (true)
            {
                System.Text.Encoding enc = System.Text.Encoding.ASCII;
                string message = enc.GetString(messageCache);

                if (message.StartsWith("[sen]") && message.Length > 10)
                {
                    float value1 = 1 - messageCache[7] / 255f;
                    float value2 = 1 - messageCache[8] / 255f;
                    float value3 = 1 - messageCache[9] / 255f;
                    messageCache = subArray(messageCache, 11);

                    MyPoint lastReceivedPosition = lastReceivedOrientation.position;
                    double lastReceivedAngle = lastReceivedOrientation.direction;
                    Vector2 pos = new Vector2(lastReceivedPosition.X*10/1024f, lastReceivedPosition.Y*10/768f);
                    Vector2 dir = new Vector2((float)Math.Cos(lastReceivedAngle), (float)Math.Sin(lastReceivedAngle));

                    Message msg1 = new Message();
                    msg1.CreateUpdateMessage(0, pos, dir,
                        new float[] { value1, value2, value3 }, new int[] { driveMotor, turnMotor }, 0);
                    communicator.SendMessage(0, msg1);
                }
                else if (message.Length > 6)
                {
                    int end = message.IndexOf(']', 6);
                    if (end < 0)
                        break;
                    messageCache = subArray(messageCache, end + 1);
                }
                else
                    break;
            }
        }
        //----

        private void wm_WiimoteChanged(object sender, WiimoteChangedEventArgs args)
        {
            Orientation o = getOrientation(args);
            if(o != null)
                lastReceivedOrientation = o;
        }

        private void DelayedMessageThread()
        {
            while (true)
            {
                UpdateDelayedMessages();
            }
        }
        private void ParseMessage(Message nextMessage)
        {
            if (nextMessage != null)
            {
                communicator_log.WriteLog("The next message is " + nextMessage.SaveToString());
                // Here we only care about the Server to Client messages. This is found in message.cs
                // MOTOR_SET,           // Command to set the motor to specific value  Params: {int WhichMotor} {int value}    Usage: Server to Client
                // MOTOR_INC,           // Command to increment the motor setting      Params: {int WhichMotor} {int value}    Usage: Server to Client
                // MOTOR_INVERT,        // Command to invert the current motor setting Params: {int WhichMotor}                Usage: Server to Client
                int WhichMotor = -1;
                int NewSetting = -1;
                int OldSetting = -1;
                switch ((int)nextMessage.GetMessageType())
                {
                    case (int)MessageType.BURST_BEGIN:
                        receiveServerMessages = true;
                        parsedServerMessages = 0;
                        expectedServerMessages = Convert.ToInt32(nextMessage.GetField("count"));
                        break;

                    case (int)MessageType.BURST_END:
                        receiveServerMessages = false;
                        requestAlgorithmUpdate = true;
                        break;

                    case (int)MessageType.SLEEP:
                        try
                        {
                            Thread.Sleep(Convert.ToInt32(nextMessage.GetField("MS")));
                        }
                        catch (FormatException e)
                        {
                            communicator_log.WriteLog("Format Exception: " + e.Message);
                            break;
                        }
                        break;
                    case (int)MessageType.MOTOR_SET:
                        //  Only if we are receiving we should consider this message
                        if (receiveServerMessages == true)
                        {
                            try
                            {
                                int Delay = Convert.ToInt32(nextMessage.GetField("DelayMS"));
                                if (Delay > 0)
                                {
                                    DelayedMessage newDelayedMessage = new DelayedMessage(nextMessage, DateTime.Now, Delay);
                                    lock (delayedMessages)
                                    {
                                        delayedMessages.Add(newDelayedMessage);
                                    }
                                    break;
                                }
                                WhichMotor = Convert.ToInt32(nextMessage.GetField("motor_id"));
                                NewSetting = Convert.ToInt32(nextMessage.GetField("setting"));
                            }
                            catch (FormatException e)
                            {
                                communicator_log.WriteLog("Format Exception: " + e.Message);
                                parsedServerMessages++;
                                break;
                            }

                            // PUT THE ROBOT UPDATE MOTOR SETTINGS HERE---------------------------------------------

                            // Check bounds
                            if (WhichMotor == (int)MotorID.DRIVE)
                                NewSetting =    Math.Max((int)DriveMotorParams.MIN,
                                                Math.Min((int)DriveMotorParams.MAX, NewSetting));
                            else if (WhichMotor == (int)MotorID.STEERING)
                                NewSetting =    Math.Max((int)SteeringMotorParams.MIN,
                                                Math.Min((int)SteeringMotorParams.MAX, NewSetting));

                            sendMotorControl(WhichMotor, NewSetting);
                            parsedServerMessages++;
                        }


                        break;
                    case (int)MessageType.MOTOR_INC:
                        // Message params should be int for which motor and int for setting
                        if (receiveServerMessages == true)
                        {
                            try
                            {
                                int Delay = Convert.ToInt32(nextMessage.GetField("DelayMS"));
                                if (Delay > 0)
                                {
                                    DelayedMessage newDelayedMessage = new DelayedMessage(nextMessage, DateTime.Now, Delay);
                                    lock (delayedMessages)
                                    {
                                        delayedMessages.Add(newDelayedMessage);
                                    }
                                    break;
                                }
                                WhichMotor = Convert.ToInt32(nextMessage.GetField("motor_id"));
                                NewSetting = Convert.ToInt32(nextMessage.GetField("amount"));
                            }
                            catch (FormatException e)
                            {
                                communicator_log.WriteLog("Format Exception: " + e.Message);
                                parsedServerMessages++;
                                break;
                            }

                            // PUT THE NEW MOTOR SETTINGS HERE
                            OldSetting = (WhichMotor == (int)MotorID.DRIVE) ? driveMotor : turnMotor;
                            NewSetting = OldSetting + NewSetting;

                            // Check bounds
                            if (WhichMotor == (int)MotorID.DRIVE)
                                NewSetting =    Math.Max((int)DriveMotorParams.MIN,
                                                Math.Min((int)DriveMotorParams.MAX, NewSetting));
                            else if (WhichMotor == (int)MotorID.STEERING)
                                NewSetting =    Math.Max((int)SteeringMotorParams.MIN,
                                                Math.Min((int)SteeringMotorParams.MAX, (int)NewSetting));

                            sendMotorControl(WhichMotor, NewSetting);

                            parsedServerMessages++;
                        }

                        break;
                    case (int)MessageType.MOTOR_INVERT:
                        // Message params should be int for which motor and int for setting
                        if (receiveServerMessages == true)
                        {
                            try
                            {
                                int Delay = Convert.ToInt32(nextMessage.GetField("DelayMS"));
                                if (Delay > 0)
                                {
                                    DelayedMessage newDelayedMessage = new DelayedMessage(nextMessage, DateTime.Now, Delay);
                                    lock (delayedMessages)
                                    {
                                        delayedMessages.Add(newDelayedMessage);
                                        parsedServerMessages++;
                                    }
                                    break;
                                }
                                WhichMotor = Convert.ToInt32(nextMessage.GetField("motor_id"));
                            }
                            catch (FormatException e)
                            {
                                communicator_log.WriteLog("Format Exception: " + e.Message);
                                parsedServerMessages++;
                                break;
                            }

                            // Send the new motor controls to the robot
// Check bounds
                            if (WhichMotor == (int)MotorID.DRIVE)
                                NewSetting =    Math.Max((int)DriveMotorParams.MIN,
                                                Math.Min((int)DriveMotorParams.MAX, NewSetting));
                            else if (WhichMotor == (int)MotorID.STEERING)
                                NewSetting =    Math.Max((int)SteeringMotorParams.MIN,
                                                Math.Min((int)SteeringMotorParams.MAX, (int)NewSetting));
                            sendMotorControl(WhichMotor, NewSetting);
                            parsedServerMessages++;
                        }
                        break;
                    default:
                        break;
                }
            }
        }

        private void sendMotorControl(int motorId, int value)
        {
            if (motorId == (int)MotorID.STEERING)
            {
                port.Write(new char[] { (char)value, (char)driveMotor }, 0, 2);
                turnMotor = value;
            }
            else if (motorId == (int)MotorID.DRIVE)
            {
                port.Write(new char[] { (char)turnMotor, (char)value }, 0, 2);
                driveMotor = value;
            }
            else
                throw new Exception("Unreachable Statement");
        }

        // Return true if we have parsed a new message
        private bool UpdateDelayedMessages()
        {
            // Wait for the completion of the delayed messages (hopefully not too long!)
            for (int i = 0; i < delayedMessages.Count; i++)
            {
                DelayedMessage msg = null;
                lock (delayedMessages)
                {
                    msg = delayedMessages[i];
                }
                TimeSpan Diff = DateTime.Now - msg.GetStartTime();
                if (Diff.Milliseconds > msg.GetDelayPeriod())
                {
                    // Retrieve the next message, and change the delay to 0
                    Message nextMessage = msg.GetMessage();
                    MessageField mf = new MessageField("DelayMS");
                    mf.SetValue("0");
                    nextMessage.SetField(mf);

                    lock (delayedMessages)
                    {
                        delayedMessages.Remove(msg);
                    }

                    ParseMessage(nextMessage);
                    return true;
                }
            }
            return false;
        }
        /// <summary>
        /// This is called when the game should draw itself.
        /// </summary>
        /// <param name="gameTime">Provides a snapshot of timing values.</param>
        protected override void Draw(GameTime gameTime)
        {
            //Clear the device
            graphics.GraphicsDevice.Clear(Color.CornflowerBlue);

            instructionsString1.Draw();
            instructionsString2.Draw();

            base.Draw(gameTime);
        }

        //------------------INTERNAL DETAILS--------------------------------------------
        private static Orientation getOrientation(WiimoteChangedEventArgs wiistate)
        {
            MyPoint[] points = new MyPoint[3];
            for (int i = 0; i < 3; i++)
            {
                points[i] = getIRPosition(wiistate, i);
                if (points[i] == null)
                    return null;
            }

            double[] distances = new double[3];
            for (int i = 0; i < 3; i++)
                distances[i] = distanceSq(points[i], points[(i + 1) % 3]);

            //Identify Closest Two Points
            MyPoint backA;
            MyPoint backB;
            MyPoint front;

            if (distances[0] <= distances[1] &&
              distances[0] <= distances[2])
            {
                backA = points[0];
                backB = points[1];
                front = points[2];
            }
            else if (distances[1] <= distances[0] &&
              distances[1] <= distances[2])
            {
                backA = points[1];
                backB = points[2];
                front = points[0];
            }
            else
            {
                backA = points[2];
                backB = points[0];
                front = points[1];
            }

            MyPoint position = new MyPoint(
              (points[0].X + points[1].X + points[2].X) / 3,
              (points[0].Y + points[1].Y + points[2].Y) / 3);

            double direction = getAngle(
              new MyPoint((backA.X + backB.X) / 2, (backA.Y + backB.Y) / 2),
              front);

            return new Orientation(position, direction);
        }

        private static double distanceSq(MyPoint pointA, MyPoint pointB)
        {
            int dx = pointB.X - pointA.X;
            int dy = pointB.Y - pointA.Y;
            return dx * dx + dy * dy;
        }

        private static double getAngle(MyPoint from, MyPoint to)
        {
            return Math.Atan2(
              to.Y - from.Y,
              to.X - from.X);
        }

        private static MyPoint getIRPosition(WiimoteChangedEventArgs wiistate, int num)
        {
            IRSensor sensor = wiistate.WiimoteState.IRState.IRSensors[num];
            if (sensor.Found)
                return new MyPoint(sensor.RawPosition.X, 768 - sensor.RawPosition.Y);
            return null;
        }
    }

    public class MyPoint
    {
        public int X;
        public int Y;

        public MyPoint(int aX, int aY)
        {
            X = aX;
            Y = aY;
        }

        public MyPoint(MyPoint p)
        {
            X = p.X;
            Y = p.Y;
        }
    }

    public class Orientation
    {
        public MyPoint position;
        public double direction;

        public Orientation(MyPoint aPosition, double aDirection)
        {
            position = aPosition;
            direction = aDirection;
        }
    }
}
